#ifndef ALGORITHM_COLLECTOR_H__
#define ALGORITHM_COLLECTOR_H__

#ifdef __cplusplus
extern "C" {
#endif

#include <stdio.h>

//======================================== 二分法查找 ========================================================================
void *ks_bsearch(const void *key, const void *base, size_t num, size_t size,
	      int (*cmp)(const void *key, const void *elt), int (*cmp_last2)(const void *key, const void *s, const void *e) );

typedef int (*bsearch_cmp_func_t)(const void * a, const void * b);

#define DECLARE_BSEARCH_CMP(type)    \
int cmp_equ_##type(const void * a, const void * b);
DECLARE_BSEARCH_CMP(char)
DECLARE_BSEARCH_CMP(int8_t)
DECLARE_BSEARCH_CMP(uint8_t)
DECLARE_BSEARCH_CMP(int16_t)
DECLARE_BSEARCH_CMP(uint16_t)
DECLARE_BSEARCH_CMP(int32_t)
DECLARE_BSEARCH_CMP(uint32_t)
DECLARE_BSEARCH_CMP(long)
DECLARE_BSEARCH_CMP(int64_t)
DECLARE_BSEARCH_CMP(uint64_t)

#define get_bsearch_func(x) _Generic((x), \
    char:       cmp_equ_char, \
    int8_t:     cmp_equ_int8_t, \
    uint8_t:    cmp_equ_uint8_t, \
    int16_t:    cmp_equ_int16_t, \
    uint16_t:   cmp_equ_uint16_t, \
    int32_t:    cmp_equ_int32_t, \
    uint32_t:   cmp_equ_uint32_t, \
    long:       cmp_equ_long,    \
    unsigned long:      cmp_equ_uint32_t,    \
    int64_t:    cmp_equ_int64_t, \
    uint64_t:   cmp_equ_uint64_t, \
    default:    NULL )

#define binary_search(first, last, target, ...) \
({  \
    STATIC_ASSERT(types_cmp_p(__typeof(first), __typeof(last) ), "file:" __FILE__ " line:" TO_STRING(__LINE__) " must be same type" ); \
    size_t width = sizeof(*first);  \
    size_t nelem = (size_t)(last - first); \
    __typeof(target)    target_in_binary_search = target;   \
    __typeof(first)     ret_in_binary_search;   \
    if(MY_NUM_VA_ARGS(__VA_ARGS__) == 0){   \
        ret_in_binary_search = (__typeof(first) )bsearch(&target_in_binary_search, first, nelem, width, get_bsearch_func(*first) ); \
    }else{  \
        ret_in_binary_search = (__typeof(first) )bsearch(&target_in_binary_search, first, nelem, width, GET_ARGS(__VA_ARGS__) + 0 ); \
    }   \
    printf("ret_in_binary_search: %p\n", ret_in_binary_search); \
    ret_in_binary_search;   \
})
//======================================== 二分法查找 end ========================================================================


//======================================== 三次样条插值 ===========================================================================
typedef struct spline_s *spl_t;
struct spline_s{

    int (*spline) (spl_t v);
    double (*get_val) ( spl_t v, double u);
    int (*set_spline_x)(spl_t v, double * x_cor, size_t cnt);
    int (*set_spline_y)(spl_t v, double * y_cor, size_t cnt);
    int n;
    double *x;
    double *y;
    double *b;
    double *c;
    double *d;

    int end1;       // 端点条件, 0:自由边界, 1:固定边界
    int end2;
    double slope1;  // 指定的斜率
    double slope2;

};


int spline (spl_t v);
double get_val ( spl_t v, double u);
int set_spline_x(spl_t v, double * x_cor, size_t cnt);
int set_spline_y(spl_t v, double * y_cor, size_t cnt);

#define DEFINE_SPLINE_METHOD(name)  \
int name##_$spline(void)    \
{   \
    return name.spline(&name);   \
}   \
double name##_$get_val(double u)  \
{   \
    return name.get_val(&name, u);   \
}   \
int name##_$set_spline_x(double * x_cor, size_t cnt)  \
{   \
    return name.set_spline_x(&name, x_cor, cnt);   \
}   \
int name##_$set_spline_y(double * y_cor, size_t cnt)  \
{   \
    return name.set_spline_y(&name, y_cor, cnt);   \
}

/**
 *@description: 定义样条插值实例
 *@name:                实例名字
 *@num_of_points:       输入样本点个数
 *@author: Kalimdorsummer
 *@date: 2021-04-19 15:58:14
*/
#define DEFINE_SPLINE(name, num_of_points)  \
double name##_x_buf[num_of_points]; \
double name##_y_buf[num_of_points]; \
double name##_b_buf[num_of_points]; \
double name##_c_buf[num_of_points]; \
double name##_d_buf[num_of_points]; \
struct spline_s name = {    \
    .spline = spline,  \
    .get_val = get_val,     \
    .set_spline_x = set_spline_x, \
    .set_spline_y = set_spline_y, \
    .n = num_of_points, \
    .x = name##_x_buf,  \
    .y = name##_y_buf,  \
    .b = name##_b_buf,  \
    .c = name##_c_buf,  \
    .d = name##_d_buf,  \
};   \
DEFINE_SPLINE_METHOD(name);

int spline_init(spl_t v, size_t num_of_points);
int spline_destroy(spl_t v);
//======================================== 三次样条插值 end ===========================================================================


//======================================== polyfit ====================================================================
typedef struct polyfit_s* polyfit_t;
struct polyfit_s{

    void (*polyfit) ( polyfit_t v, double x[], double y[], double a[] );

    size_t data_num;
    size_t poly_n;

    double *tempx;
    double *tempy;
    double *sumxx;
    double *sumxy;
    double *ata;
};


#define DEFINE_POLYFIT_METHOD(name)  \
void name##_$polyfit(double x[], double y[], double a[])    \
{   \
    if ( name.polyfit == polyfit ) {    \
        name.polyfit(&name, x, y, a);   \
    }   \
}


#define DEFINE_POLYFIT(poly_name, poly_data_num, poly_order_n)  \
static double poly_name##_tempx[poly_data_num];  \
static double poly_name##_tempy[poly_data_num];  \
static double poly_name##_sumxx[poly_order_n * 2 + 1];    \
static double poly_name##_sumxy[poly_order_n + 1];    \
static double poly_name##_ata[( poly_order_n + 1 ) * ( poly_order_n + 1 )]; \
struct polyfit_s poly_name = {  \
    .polyfit = polyfit, \
    .data_num = poly_data_num,   \
    .poly_n = poly_order_n,   \
    .tempx = poly_name##_tempx,	\
    .tempy = poly_name##_tempy,	\
    .sumxx = poly_name##_sumxx,	\
    .sumxy = poly_name##_sumxy,	\
    .ata = poly_name##_ata,	\
};  \
DEFINE_POLYFIT_METHOD(poly_name);

void polyfit ( polyfit_t v, double x[], double y[], double a[] );
int polyfit_init(polyfit_t v, size_t poly_data_num, size_t poly_order_n);
int polyfit_destroy(polyfit_t v);
double get_polyfunc_val(double x, double *a, size_t a_cnt);
//======================================== polyfit end ====================================================================

//======================================== 删除数组重复元素 ==============================================================
#define DEL_SAME(data_in, ele_cnt) \
({  \
    size_t ret_new_n = ele_cnt; \
    ret_new_n = delete_same(data_in, ele_cnt, sizeof(data_in[0]), NULL); \
    ret_new_n;  \
})
size_t delete_same(void *data_in, size_t n, size_t size, int (*cmp_same)(void *search_val, void *search_same) );
//======================================== 删除数组重复元素 end ==============================================================


//======================================== 计算标准差 ==================================================================
#define CALC_STD_DEVIATION(data_in, data_cnt)   \
({  \
	size_t data_cnt_tmp = data_cnt; \
	float sum = 0.0, mean, standardDeviation = 0.0; \
	for(int i = 0; i < data_cnt_tmp; i++){    \
		sum += data_in[i]; \
	}   \
	mean = sum / data_cnt_tmp;  \
		\
	for(int i = 0; i < data_cnt_tmp; i++){    \
		standardDeviation += pow(data_in[i] - mean, 2);    \
	}   \
		\
	float tmp = standardDeviation / data_cnt_tmp;   \
	standardDeviation = sqrt(tmp); \
	standardDeviation;  \
})
//======================================== 计算标准差 end =======================================


//======================================== 卡尔曼滤波 ===========================================

typedef struct kalman_s     *kalman_t;
struct kalman_s{
#ifdef __cplusplus
    kalman_s(float MeasureNoise_R, float ProcessNiose_Q, float init_state);
    ~kalman_s();
#endif // __cplusplus

    float (*kalman_filter)(kalman_t v, const float data_in);

    float Q;
    float R;
    float x_last;
	float x_mid;
	float x_now;

	float p_last;
	float p_mid;
	float p_now;

	float kg;
};

/*
    Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏, 估计不确定度的最小值,
    R:测量噪声，R增大，动态响应变慢，收敛稳定性变好, 测量不确定度
*/
void kalman_filter_init(kalman_t v, float MeasureNoise_R, float ProcessNiose_Q, float init_state);


//======================================== 卡尔曼滤波 end ===========================================

//======================================== 开方 ==============================================
void set_sqrt_precision_lvl(unsigned int lvl);
float ks_sqrt ( float x );
float ks_sqrt_inv( float number );
//======================================== 开方 end ==============================================



#ifdef __cplusplus
}
#endif

#endif
